'''
####
Date : 2024-10-17
Notes: 示教器server
Test: 通讯格式
1. 修改代码执行部分代码---解析代码长度(通讯接收缓存)
    client_socket.recv(10240, 0x40),第二个参数 阻塞/与否
2. 增添视觉通讯功能代码
3. KBot通讯功能完善
4. 0.1.0版本--2024.10.29
####

Modify: 
1. kata新库更换--详见self.robot_control类


####
'''

# -*- coding: utf-8 -*-

import socket
import struct
import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp

class process_Master():

    def __init__(self):
        super().__init__()
        self.host = socket.gethostname()
        self._master_init()

    def _master_init(self):
        self.master_ = modbus_tcp.TcpMaster(host=self.host)

    def get_All_DI(self, value):
        try:
            value_tmp = 37 + value
            #### quantity_of_x = 11
            value_dst = self.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=value_tmp, quantity_of_x=1)
            return value_dst[0]
        
        except Exception as e:
            print(e) 

    def get_All_DO(self, value):
        try:
            value_tmp = 28 + value
            #### quantity_of_x = 9
            value_dst = self.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=value_tmp, quantity_of_x=1)
            return value_dst[0]
        
        except Exception as e:
            print(e)
        
    def get_Motor(self, value):
        try:
            value = 47 + value
            #### quantity_of_x = 4
            value = self.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address=value, quantity_of_x=1)
            return value[0]
        
        except Exception as e:
            print(e)

    def read_status(self, offset):
        value = self.master_.execute(1, cst.READ_HOLDING_REGISTERS, offset, 1)
        return value[0] if value else None

    def read_robot_type(self, offset):
        value = self.master_.execute(1, cst.READ_HOLDING_REGISTERS, offset, 1)
        return value[0] if value else None

    def read_pose_floats(self, starting_address):
        motion = self.master_.execute(1, cst.READ_HOLDING_REGISTERS, starting_address, 26)
        if not motion:
            return []

        floats = []
        for i in range(0, 26, 2):
            if i + 1 < len(motion):
                floats.append(self.int2float(motion[i], motion[i+1]))
            else:
                print("Error: Not enough integers to convert to floats.")
                break
        return floats

    def get_status(self,robotNum):
        try:
            if robotNum == 1:
                robot1_status = self.read_status(0)
                return robot1_status
            elif robotNum == 2:
                robot2_status = self.read_status(60)
                return robot2_status
            elif robotNum == 3:
                robot3_status = self.read_status(100)
                return robot3_status
        
        except Exception as e:
            print(e)
    
    def get_robotType(self,robotNum):
        try:
            if robotNum == 1:
                robot1_type = self.read_robot_type(27)
                return robot1_type
            elif robotNum == 2:
                robot2_type = self.read_robot_type(87)
                return robot2_type
            elif robotNum == 3:
                robot3_type = self.read_robot_type(127)
                return robot3_type
            
        except Exception as e:
            print(e)
    
    def get_pose_floats(self, robotNum):
        try:
            if robotNum == 1:
                robot1_pose = self.read_pose_floats(1)
                return robot1_pose
            elif robotNum == 2:
                robot2_pose = self.read_pose_floats(61)
                return robot2_pose
            elif robotNum == 3:
                robot3_pose = self.read_pose_floats(101)
                return robot3_pose
        
        except Exception as e:
            print(e)

    def get_hold_register(self,address, num):
        value = self.master_.execute(1, cst.READ_HOLDING_REGISTERS, address, num)
        return value if value else None

    def get_coils_register(self, address, num):
        value = self.master_.execute(1, cst.READ_COILS, address, num)
        return value if value else None
    
    ### 读取寄存器值，转换为float显示应用
    def int2float(self,a,b):
        value=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            value=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
            if value is None:
                return value
            if isinstance(value, float):
                # 精确到小数点后两位数
                return round(value , 2)
            else:
                return value
        except BaseException as e:
            print(e)
        return value
